#ifndef SOPHUS_INTERPOLATE_DETAILS_HPP
#define SOPHUS_INTERPOLATE_DETAILS_HPP

#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"

namespace Sophus
{
  namespace interp_details
  {
    template <class Group>
    struct Traits;

    template <class Scalar>
    struct Traits<SO2<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(SO2<Scalar> const &foo_T_bar)
      {
        using std::abs;
        Scalar angle = foo_T_bar.log();
        return abs(abs(angle) - Constants<Scalar>::pi()) <
               Constants<Scalar>::epsilon();
      }
    };

    template <class Scalar>
    struct Traits<RxSO2<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(RxSO2<Scalar> const &foo_T_bar)
      {
        return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
      }
    };

    template <class Scalar>
    struct Traits<SO3<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(SO3<Scalar> const &foo_T_bar)
      {
        using std::abs;
        Scalar angle = foo_T_bar.logAndTheta().theta;
        return abs(abs(angle) - Constants<Scalar>::pi()) <
               Constants<Scalar>::epsilon();
      }
    };

    template <class Scalar>
    struct Traits<RxSO3<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(RxSO3<Scalar> const &foo_T_bar)
      {
        return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
      }
    };

    template <class Scalar>
    struct Traits<SE2<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(SE2<Scalar> const &foo_T_bar)
      {
        return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
      }
    };

    template <class Scalar>
    struct Traits<SE3<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(SE3<Scalar> const &foo_T_bar)
      {
        return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
      }
    };

    template <class Scalar>
    struct Traits<Sim2<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(Sim2<Scalar> const &foo_T_bar)
      {
        return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
            foo_T_bar.rxso2().so2());
        ;
      }
    };

    template <class Scalar>
    struct Traits<Sim3<Scalar>>
    {
      static bool constexpr supported = true;

      static bool hasShortestPathAmbiguity(Sim3<Scalar> const &foo_T_bar)
      {
        return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
            foo_T_bar.rxso3().so3());
        ;
      }
    };

  } // namespace interp_details
} // namespace Sophus

#endif // SOPHUS_INTERPOLATE_DETAILS_HPP
